#include <math.h>
#include <vector>
#include <list>
#include "std_msgs/String.h"
#include <sstream>
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>

#include "geometry_msgs/Point32.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/PointStamped.h"

#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/PointCloud.h"



#ifndef PI
  #define PI 3.1415926535897
#endif

// for async threads
#include <boost/thread.hpp>

using namespace std;


// global ROS subscriber handles
ros::Subscriber subscriberHokuyo;
sensor_msgs::LaserScan LaserSensorStatus;
/*--------------------------- ROS callbacks -----------------------------*/
void hokuyoLaserScannercallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
  int length = msg->ranges.size();
  float inc = msg->angle_increment;
  float range_min = msg->range_min;
  float range_max = msg->range_max;
  int num_valid = 0;
  
  for(int i = 0; i < length; i++)
  {
    #ifdef FORCED_MAX_RANGE
      if(msg->ranges[i] >= range_min || msg->ranges[i] == 0) // zero means no reading, which could happen in a large open area
        num_valid++;      
    #else
      if(msg->ranges[i] >= range_min && msg->ranges[i] <= range_max)
        num_valid++;
    #endif
  }
    
  if(num_valid <= 0)
    return;
  else if(num_valid >0){
    LaserSensorStatus.angle_min = msg->angle_min;
    LaserSensorStatus.angle_max = msg->angle_max;
    LaserSensorStatus.angle_increment = msg->angle_increment;
    LaserSensorStatus.time_increment = msg->time_increment;  
    LaserSensorStatus.scan_time = msg->scan_time;
    LaserSensorStatus.range_min = msg->range_min;
    LaserSensorStatus.range_max = msg->range_max;
    LaserSensorStatus.ranges = msg->ranges ;
    LaserSensorStatus.intensities = msg->intensities; 
   
    
  }
}

// for finding the average range
double avgRange(){
  int length = LaserSensorStatus.ranges.size();
  double avg = 0;
  for(int i = 0; i < length; i++)
  {
     avg = LaserSensorStatus.ranges[i]+avg;
   }
   return avg/length;
}
// for finding the average intensities
double avgIntensities(){
  double avg = 0;
  int length = LaserSensorStatus.intensities.size();
  cout << " The length of intensity scan "<<length <<endl;
  for(int i = 0; i < length; i++)
  {//    if(LaserSensorStatus.intensities[i] !=0){
    cout << " The Intensity Value Number "<< i << " is :" <<LaserSensorStatus.intensities[i] <<endl;
    //}
   // avg = LaserSensorStatus.intensities[i]+avg;
   }
   return avg/length;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "listenerHokuyo");
  ros::NodeHandle n;
  ROS_INFO("Hokuyo Node Listener subscriber");
  //ros::Subscriber sub = n.subscribe("odom", 10, odomCallback);
  subscriberHokuyo = n.subscribe("scan", 1, hokuyoLaserScannercallback);
  
  char c;
      
      ros::AsyncSpinner vnStatusSpinner(1);
      vnStatusSpinner.start();
      ros::Rate threadRate(1);
   //ros::spin();
   //int i=0;
     while (ros::ok()){ // rosok bracket
     
     cout <<"Enter a Character and press Enter to move. Enter q to Quit ..." <<endl;
     cin >> c;
     if (c=='q') return 1;
      if(c=='p'){
	    cout<< "The recieved data is ........."<<endl;
	    cout << " Hokuyo: Angle Minimum       " <<LaserSensorStatus.angle_min <<endl;
	    cout << " Hokuyo: Angle Maximum       " <<LaserSensorStatus.angle_max <<endl;
	    cout << " Hokuyo: Angle Increment     " <<LaserSensorStatus.angle_increment <<endl;
	    cout << " Hokuyo: Time Increment      " <<LaserSensorStatus.time_increment <<endl;
	    cout << " Hokuyo: Scanning Time       " <<LaserSensorStatus.scan_time <<endl;
	    cout << " Hokuyo: Minimum Range       " <<LaserSensorStatus.range_min <<endl;
	    cout << " Hokuyo: Maximum Range       " <<LaserSensorStatus.range_max <<endl;
	    cout << " Hokuyo: Range Size          " <<LaserSensorStatus.ranges.size() <<endl;
	    cout << " Hokuyo: Range Average       " <<avgRange() <<endl;
	    cout << " Hokuyo: Intensities Average " <<avgIntensities() <<endl;
	    threadRate.sleep();

	
      }
	if(c=='c'){
	   threadRate.sleep();
	
	}
 // ros::spin();
      }
      
  return 0;
}
